#include <engine.h>
#include <defs.h>
#include <user.h>
#include <string.h>
#include <sensor.h>
#include <printer.h>
#include <utils.h>
#include <track.h>
#include <routefinder.h>
#include <messages.h>
#include <pathplanner.h>
#include <io.h>
#include <tron.h>

/* MAIN ENGINE SERVER STUFF */

void EngineSpeedNotifier() {
  engine_req msg;
  msg.type = MSG_ENGINE_SPEED;
  int train_number;

  int manager_tid;
  Receive(&manager_tid, (char*)&train_number, sizeof(int));
  Reply(manager_tid, NULL, 0);

  for ( ; ; ) {
    msg.speed = WaitTrainSpeed(train_number);
    Send(manager_tid, (char*)&msg, sizeof(engine_req), NULL, 0);
  }
  Panic("EngineSpeedNotifier", "Chuck Norris!");
}

void EngineTickNotifier() {
  engine_req msg;
  msg.type = MSG_ENGINE_TICK;

  int server_tid;
  Receive(&server_tid, NULL, 0);
  Reply(server_tid, NULL, 0);

  for ( ; ; ) {
    AwaitEvent(EVENT_TIMER, NULL, 0);
    Send(server_tid, (char*)&msg, sizeof(engine_req), NULL, 0);
  }
  Panic("EngineTickNotifier", "Chuck Norris!");
}

void EngineSensorNotifier() {
  engine_req msg;
  msg.type = MSG_ENGINE_UPDATE_SENSOR;
  char sensor_data[10];
  int server_tid;
  Receive(&server_tid, NULL, 0);
  Reply(server_tid, NULL, 0);

  for ( ; ; ) {
    AwaitEvent(EVENT_NEW_SENSOR, sensor_data, 10);
    SensorToExternal(sensor_data, msg.last_sensor);
    Send(server_tid, (char*)&msg, sizeof(engine_req), NULL, 0);
  }
  Panic("EngineSensorNotifier", "Chuck Norris!");
}

void EngineInit(engine *e, engine_init init_msg) {
  e->train_number = init_msg.train; 
  
  // speed state
  e->speed = 0;
  e->last_speed = 0;

  // expected (extrapolated) state
  e->expect_dist_prev_sensor = UNDEFINED;
  e->expect_dist_prev = UNDEFINED;
  e->expect_prev = UNDEFINED;
  e->expect_curr = init_msg.starting_landmark;
  e->expect_next = init_msg.next_landmark;
  // TODO: what do we fill in for this distance? Will the automatic
  // calibration take this into account?
  e->expect_dist_next = 500;

  // actual (measured) state
  e->actual_curr = 66;
  e->actual_speed_prev = 0;
  e->actual_ticks_prev = Time();
  e->state = ENGINE_STOPPED;

  // predictions - some hardcoded values for now
  e->first_prediction.dest = 0;
  e->first_prediction.distance = 500;
  e->second_prediction.dest = 22;
  e->second_prediction.distance = 1000;

  // velocity/accel tables
  e->vel_table_accel[0] = 0*UM_PER_MM;
  e->vel_table_accel[1] = 37*UM_PER_MM;
  e->vel_table_accel[2] = 74*UM_PER_MM;
  e->vel_table_accel[3] = 124*UM_PER_MM;
  e->vel_table_accel[4] = 178*UM_PER_MM;
  e->vel_table_accel[5] = 221*UM_PER_MM;
  e->vel_table_accel[6] = 268*UM_PER_MM;
  e->vel_table_accel[7] = 311*UM_PER_MM;
  e->vel_table_accel[8] = 357*UM_PER_MM;
  e->vel_table_accel[9] = 399*UM_PER_MM;
  e->vel_table_accel[10] = 442*UM_PER_MM;
  e->vel_table_accel[11] = 481*UM_PER_MM;
  e->vel_table_accel[12] = 520*UM_PER_MM;
  e->vel_table_accel[13] = 549*UM_PER_MM;
  e->vel_table_accel[14] = 611*UM_PER_MM;
  
  e->vel_table_decel[0] = 0*UM_PER_MM;
  e->vel_table_decel[1] = 37*UM_PER_MM;
  e->vel_table_decel[2] = 74*UM_PER_MM;
  e->vel_table_decel[3] = 124*UM_PER_MM;
  e->vel_table_decel[4] = 178*UM_PER_MM;
  e->vel_table_decel[5] = 221*UM_PER_MM;
  e->vel_table_decel[6] = 268*UM_PER_MM;
  e->vel_table_decel[7] = 311*UM_PER_MM;
  e->vel_table_decel[8] = 357*UM_PER_MM;
  e->vel_table_decel[9] = 399*UM_PER_MM;
  e->vel_table_decel[10] = 442*UM_PER_MM;
  e->vel_table_decel[11] = 481*UM_PER_MM;
  e->vel_table_decel[12] = 520*UM_PER_MM;
  e->vel_table_decel[13] = 549*UM_PER_MM;
  e->vel_table_decel[14] = 611*UM_PER_MM;

  // TODO: actually measure stop distances for 1-5
  e->stop_dist_accel[0] = 0*UM_PER_MM;
  e->stop_dist_accel[1] = 75*UM_PER_MM;
  e->stop_dist_accel[2] = 150*UM_PER_MM;
  e->stop_dist_accel[3] = 225*UM_PER_MM;
  e->stop_dist_accel[4] = 300*UM_PER_MM;
  e->stop_dist_accel[5] = 375*UM_PER_MM;
  e->stop_dist_accel[6] = 450*UM_PER_MM;
  e->stop_dist_accel[7] = 490*UM_PER_MM;
  e->stop_dist_accel[8] = 600*UM_PER_MM;
  e->stop_dist_accel[9] = 700*UM_PER_MM;
  e->stop_dist_accel[10] = 780*UM_PER_MM;
  e->stop_dist_accel[11] = 890*UM_PER_MM;
  e->stop_dist_accel[12] = 960*UM_PER_MM;
  e->stop_dist_accel[13] = 1090*UM_PER_MM;
  e->stop_dist_accel[14] = 1160*UM_PER_MM;

  e->stop_dist_decel[0] = 0*UM_PER_MM;
  e->stop_dist_decel[1] = 80*UM_PER_MM;
  e->stop_dist_decel[2] = 160*UM_PER_MM;
  e->stop_dist_decel[3] = 240*UM_PER_MM;
  e->stop_dist_decel[4] = 320*UM_PER_MM;
  e->stop_dist_decel[5] = 400*UM_PER_MM;
  e->stop_dist_decel[6] = 480*UM_PER_MM;
  e->stop_dist_decel[7] = 560*UM_PER_MM;
  e->stop_dist_decel[8] = 680*UM_PER_MM;
  e->stop_dist_decel[9] = 770*UM_PER_MM;
  e->stop_dist_decel[10] = 890*UM_PER_MM;
  e->stop_dist_decel[11] = 950*UM_PER_MM;
  e->stop_dist_decel[12] = 1010*UM_PER_MM;
  e->stop_dist_decel[13] = 1110*UM_PER_MM;
  e->stop_dist_decel[14] = 1180*UM_PER_MM;

  // ticks since last print
  e->last_print_ticks = 0;

  // line to print status on
  e->status_line = init_msg.status_line;
}

void EngineNotify(engine *e) {
  landmark_event event;
  event.train = e->train_number;
  event.landmark = e->expect_curr;
  
  char name2[5];
  InternalToExternal(event.landmark, name2); 
  PrintAt(1, 24, "engine: %s\r\n", name2);
  NotifyEvent(EVENT_LANDMARK, (char*)&event, sizeof(landmark_event));
}

void EngineNextLandmark(engine *e) {
  e->expect_prev = e->expect_curr;
  e->expect_curr = e->expect_next;
  track_edge next;
  GetNextLandmark(e->expect_prev, e->expect_curr, &next);
  EngineNotify(e);
  e->expect_next = next.dest;
  e->expect_dist_next = next.distance*UM_PER_MM;
}

/*
void EngineDisplayActual(engine *e, int dx) {
  char landmark[5];
  InternalToExternal(e->actual_curr, landmark);
  int vel = (e->speed >= e->last_speed) ?
      e->vel_table_accel[e->speed] : e->vel_table_decel[e->speed];
  PrintAt(0, 4,
      "sensor %s: actual %d mm, expect %d mm (%d) @ vel %d mm/s (speed %d)",
      landmark, dx, e->expect_dist_prev_sensor/UM_PER_MM,
      (e->expect_dist_prev_sensor/UM_PER_MM - dx), vel/UM_PER_MM, e->speed);
}
*/

void EngineDisplayExpect(engine *e) {
  char landmark[5];
  InternalToExternal(e->expect_curr, landmark);
  PrintAt(0, e->status_line, "%s + %d mm (state = %d)", landmark,
      e->expect_dist_prev/UM_PER_MM, e->state);
}

void EngineUpdateSpeed(engine *e, int new_speed) {
  int accel = (e->speed >= e->last_speed);
  e->last_speed = e->speed;
  e->speed = new_speed;
  if (e->speed > 0)
    e->state = ENGINE_RUNNING;
  else if (e->state != ENGINE_STOPPED) {
    if (accel) {
      e->distance_to_stop = e->stop_dist_accel[e->last_speed];
      e->stopping_vel = e->vel_table_accel[e->last_speed];
    } else {
      e->distance_to_stop = e->stop_dist_decel[e->last_speed];
      e->stopping_vel = e->vel_table_decel[e->last_speed];
    }
    // compute stopping time and deceleration
    e->stopping_decel = (e->stopping_vel*e->stopping_vel)/
        (2*e->distance_to_stop);
    e->state = ENGINE_STOPPING;
  }
}

void EngineUpdateReverse(engine *e) {
  int temp = e->expect_prev;
  e->expect_prev = e->expect_next;
  e->expect_next = temp;
  e->expect_dist_prev = e->expect_dist_next - e->expect_dist_prev;
}

void EngineSensorKinematics(engine *e, int v) {
  // update velocity table
  if (e->speed == 0)
    return;
  if (e->speed >= e->last_speed) {
    e->vel_table_accel[e->speed] = (ENGINE_LEARNING_RATE * v + 
        (100 - ENGINE_LEARNING_RATE) * e->vel_table_accel[e->speed] + 50) / 100;
  } else if (e->speed < e->last_speed) {
    e->vel_table_decel[e->speed] = (ENGINE_LEARNING_RATE * v + 
        (100 - ENGINE_LEARNING_RATE) * e->vel_table_decel[e->speed] + 50) / 100;
  }
}

void EngineUpdateSensor(engine *e, char *new_sensor) {
  // HACK: if we're headed for a dead end of the track, stop the train.
  /*
  if (StringCompare(new_sensor, "C8", 5) ||
      StringCompare(new_sensor, "C3", 5) ||
      StringCompare(new_sensor, "A6", 5) ||
      StringCompare(new_sensor, "A7", 5) ||
      StringCompare(new_sensor, "A9", 5) ||
      (StringCompare(new_sensor, "C14", 5) && GetSwitchState(11) == 'S' &&
      GetSwitchState(12) == 'S')) {
    Stop();
    Tr(e->train_number, 0);
    Rv(e->train_number);
    Go();
  }
  */
  int sensor_id = ExternalToInternal(new_sensor);
  int T = Time();
  int dx;
  if (sensor_id == e->first_prediction.dest)
    dx = e->first_prediction.distance;
  else if (sensor_id == e->second_prediction.dest)
    dx = e->second_prediction.distance;
  else {
    PrintAt(1, 24, "Train %d is LOST\r\n", e->train_number);
    // TODO: pick something marginally more intelligent to do here
    return;
  }
  if (e->state != ENGINE_STOPPING && e->state != ENGINE_STOPPED) {
    // dynamically update velocity tables
    int dt = T-e->actual_ticks_prev;
    int v = TICKS_PER_SEC*UM_PER_MM*dx/dt;
    EngineSensorKinematics(e, v);
    e->state = ENGINE_RUNNING;  
    e->actual_curr = sensor_id;
  }
  // get next two sensors in current direction
  int sensor_bit = Atoi(new_sensor+1);
  int sensor_direction = sensor_bit&1 ? SENSOR_FORWARD : SENSOR_BACK;
  track_edge next_predictions[2];
  GetNextSensors(sensor_id, sensor_direction, 2, next_predictions);
  e->first_prediction = next_predictions[0];
  e->second_prediction = next_predictions[1];
  // reset expected state
  e->expect_dist_prev_sensor = 0;
  e->expect_dist_prev = 0;
  track_edge next;
  
  if (e->expect_next != sensor_id)
    EngineNextLandmark(e);
  GetNextDirectedLandmark(sensor_id, sensor_direction, &next);
  e->expect_curr = sensor_id;
  e->expect_next = next.dest;
  e->expect_dist_next = next.distance;
  EngineNotify(e);
  // reset last sensor state
  e->actual_speed_prev = e->speed;
  e->actual_ticks_prev = T;
}

void EngineTickKinematics(engine *e) {
  if (e->state == ENGINE_STOPPING) {
    e->stopping_vel -= e->stopping_decel/TICKS_PER_SEC;
    e->expect_dist_prev_sensor += e->stopping_vel/TICKS_PER_SEC;
    e->expect_dist_prev += e->stopping_vel/TICKS_PER_SEC;
    e->distance_to_stop -= e->stopping_vel/TICKS_PER_SEC;
    if (e->distance_to_stop <= 0 || e->stopping_vel <= 0) {
      e->state = ENGINE_STOPPED;
      e->stopping_vel = 0;
      e->distance_to_stop = 0;
    }
  } else {
    int vel = (e->speed >= e->last_speed) ?
      e->vel_table_accel[e->speed] : e->vel_table_decel[e->speed];
    e->expect_dist_prev_sensor += vel/TICKS_PER_SEC;
    e->expect_dist_prev += vel/TICKS_PER_SEC;
  }
}

void EngineTick(engine *e) {
  EngineTickKinematics(e);
  // if we've passed the next landmark, ask for another prediction
  // HACK: if a sensor is next, don't get new predictions
  if ((e->expect_next >= 40) && (e->expect_dist_prev >= e->expect_dist_next)) {
    e->expect_dist_prev -= e->expect_dist_next;
    EngineNextLandmark(e);
  }
  // only print out every 3 ticks (150 ms) - the user is unlikely to care
  // about more frequent updates
  e->last_print_ticks++;
  if (e->last_print_ticks == 3) {
    e->last_print_ticks = 0;
    EngineDisplayExpect(e);
  }
}

void EngineManager() {
 
  // notify parent
  engine_init init_msg;
  int parent_tid;
  Receive(&parent_tid, (char*)&init_msg, sizeof(engine_init));
  Reply(parent_tid, NULL, 0);
  
  engine e;
  EngineInit(&e, init_msg);
  
  engine_req msg;
  engine_reply reply;
  engine_reply path_reply;
  
  int sender_tid, tron_tid;
  
  // create notifiers
  int tick_notifier_tid = Create(5, &EngineTickNotifier, "EngineTickNotifier");
  int sensor_notifier_tid = Create(5, &EngineSensorNotifier, "EngineSensorNotifier");
  int speed_notifier_tid = Create(5, &EngineSpeedNotifier, "EngineSpeedNotifier");
  //int reverse_notifier_tid = Create(5, &EngineReverseNotifier, "EngineReverseNotifier");
  
  Send(tick_notifier_tid, NULL, 0, NULL, 0);
  Send(sensor_notifier_tid, NULL, 0, NULL, 0);
  Send(speed_notifier_tid, (char*)&(e.train_number), sizeof(int), NULL, 0);
  //Send(reverse_notifier_tid, (char*)&(e.train_number), sizeof(int), NULL, 0);
  
  char buf[MAX_NAME_LEN];
  SPrintf(buf, "engine-manager-%d", e.train_number);
  RegisterAs(buf);
  
  for ( ; ; ) {
    Receive(&sender_tid, (char*)&msg, sizeof(engine_req));
    switch (msg.type) {
    case MSG_ENGINE_UPDATE_SENSOR:
      EngineUpdateSensor(&e, msg.last_sensor);
      Reply(sender_tid, NULL, 0);
      break;
    case MSG_ENGINE_TICK:
      EngineTick(&e);
      Reply(sender_tid, NULL, 0);
      break;
    case MSG_ENGINE_SPEED:
      EngineUpdateSpeed(&e, msg.speed);
      Reply(sender_tid, NULL, 0);
      break;
    case MSG_ENGINE_WHERE:
      reply.result = e.expect_curr;
      Reply(sender_tid, (char*)&reply, sizeof(engine_reply));
      break;
    case MSG_ENGINE_REVERSE:
      EngineUpdateReverse(&e);
      Reply(sender_tid, NULL, 0);
      break;
    case MSG_ENGINE_NEXT_LANDMARK:
      reply.result = e.expect_next;
      Reply(sender_tid, (char*)&reply, sizeof(engine_reply));
      break;
    case MSG_ENGINE_PLAY:
      tron_tid = Create(5, &Tron, "Tron");
      PlayGame(e.train_number, e.expect_curr, e.expect_next, msg.speed, tron_tid);
      Reply(sender_tid, NULL, 0);
      break;
    default:
      Panic("EngineManager", "invalid message type");
      break;
    }
  }
  Panic("EngineManager", "Chuck Norris!");
}

int WhereAmI(int train) {
  char buf[MAX_NAME_LEN];
  SPrintf(buf, "engine-manager-%d", train);
  int engine_tid = WhoIs(buf);
  engine_req msg;
  engine_reply reply;
  msg.type = MSG_ENGINE_WHERE;
  Send(engine_tid, (char*)&msg, sizeof(engine_req), (char*)&reply, sizeof(engine_reply));
  return reply.result;
}

int NextExpectedLandmark(int train) {
  char buf[MAX_NAME_LEN];
  SPrintf(buf, "engine-manager-%d", train);
  int engine_tid = WhoIs(buf);
  engine_req msg;
  engine_reply reply;
  msg.type = MSG_ENGINE_NEXT_LANDMARK;
  Send(engine_tid, (char*)&msg, sizeof(engine_req), (char*)&reply, sizeof(engine_reply));
  return reply.result;
}

void UpdateTrainReverse(int train) {
  char buf[MAX_NAME_LEN];
  SPrintf(buf, "engine-manager-%d", train);
  int engine_tid = WhoIs(buf);
  engine_req msg;
  msg.type = MSG_ENGINE_REVERSE;
  Send(engine_tid, (char*)&msg, sizeof(engine_req), NULL, 0);
}

void Play(int train, int opponent) {
  char buf[MAX_NAME_LEN];
  SPrintf(buf, "engine-manager-%d", train);
  int engine_tid = WhoIs(buf);
  engine_req msg;
  msg.type = MSG_ENGINE_PLAY;
  msg.speed = opponent;
  Send(engine_tid, (char*)&msg, sizeof(engine_req), NULL, 0);  
}
